/*! \mainpage Object Tracking in Stereo Camera Images with Particle Filters
 *
 * The project provides functions needed to calculate the pose of objects in 3D space
 * using a stereo camera. The state of the moving object is tracked by a particle filter.
 * 
 * Special retro reflective, spheric markers can be found in the stereo image pairs. With
 * the known parameters of the cameras' inner and outer orientations their position in 
 * space can be calculated. With the markers' known configuration on a locator, the pose
 * (position and orientation) of the locator can be determined. The pose is the measurement
 * value which is fed as an input to the particle filter.
 */



#ifndef _IMAGEPROCESSING_H_
#define _IMAGEPROCESSING_H_



#include "myBlob.h"

#include "Error.h"

void mexPrintMatrix(arma::mat m, char* text);

namespace OTPF {
	namespace ImgPro {

		cv::Mat importGrayscale( const char* filename );
		void thresholding( cv::Mat &src, cv::Mat &dst_thr, int threshold );
		
		double getBiLinValue(double x, double y, cv::Mat &img);

		bool sortByPerimeter (myBlob b1, myBlob b2);
		bool sortByCompactness (myBlob b1, myBlob b2);

		std::vector<cv::Point2f> findBlobBorders3( myBlob &blob, cv::Mat &img, int num_points, int threshold );
		std::vector<cv::Point2f> findBlobBorders3( CBlob &blob, cv::Mat &img, int num_points, int threshold );
		
		void eliminatePoints(arma::mat &Y, arma::mat &Yout, arma::colvec &phi, arma::colvec &center, double a, double b, double alpha, bool &did_eliminate);

	}

	namespace Fit {
		bool LevenbergMarquardt(
				arma::colvec (*func) (arma::rowvec &x, arma::mat &Y),	
				arma::mat    (*jacobian) (arma::rowvec &x),
				arma::colvec  &x,
				arma::mat     &Y,
				int max_it,
				double tolerance
			   );
			   
		bool LevenbergMarquardt(
				arma::colvec (*func) (arma::rowvec &x, arma::mat &Y),	
				arma::mat    (*jacobian) (arma::rowvec &x),
				arma::colvec  &x,
				arma::mat     &Y
			   );

		namespace Ellipse {
				/// Berechnet die Optimierungsfunktion.
				arma::colvec function(arma::rowvec& X, arma::mat& y);
				
				/// Berechnet die Jacobi-Matrix.
				arma::mat    jacobian(arma::rowvec &X);

				/// Ausgleichsellipse geometrisch optimieren		
				void fitGeometric( arma::mat Y, double alpha0, double a0, double b0, arma::colvec z0, double &alpha, double &a, double &b, arma::colvec &phi, arma::colvec &center, arma::colvec &s_center, bool &success );
				
				/// Ausgleichsellipse algebraisch optimieren.
				void fitAlgebraic( arma::mat Y, double &alpha, double &a, double &b, arma::colvec &center, arma::colvec &s_center );
					
				/// \brief Punkt auf einer Ellipse bestimmen.
				///
				arma::colvec ellipsePoint(double a, double b, double alpha, arma::colvec center, double phi);
		}

		namespace Circle {
			/// Berechnet die Optimierungsfunktion
				arma::colvec function(arma::rowvec &X, arma::mat &y);
					
				/// Berechnet die Jacobi-Matrix
				arma::mat    jacobian(arma::rowvec &X);
				
				/// Ausgleichskreis nach geometrischem Optimum berechnen
				void fitGeometric(arma::mat Y, arma::colvec z0, double r0, double &r, arma::colvec &phi, arma::colvec &center, arma::colvec &s_center, bool &success);
				
				/// Ein-Schritt Kleinste-Quadrate-Ausgleichskreis berechnen
				void fitDirectLeastSquares(arma::mat Y, double &r, arma::colvec &phi, arma::colvec &center, arma::colvec &s_center, bool &success);
		}

	}

	namespace Marker {
		arma::mat homologPoints(arma::mat &setA_in, arma::mat &setB_in, double band);
		
		void bundleAdjustment(	arma::colvec		&xout,		// 3x1 vector
								arma::colvec		&sout,		// 3x1 vector
								arma::colvec		&x0,		// 3x1 vector
								arma::colvec		&centerA,	// 2x1 vector
								arma::colvec		&centerB,	// 2x1 vector
								arma::colvec		&sA,
								arma::colvec		&sB,
								arma::colvec		&c,
								arma::colvec 		&a1,
								arma::colvec 		&a2,
								arma::colvec 		&a3,
								arma::colvec 		&b1,
								arma::colvec 		&b2,
								arma::colvec 		&c1,
								arma::colvec 		&c2,
								arma::colvec 		&ppx,
								arma::colvec 		&ppy,
								arma::colvec 		&r0,
								arma::colvec 		&esx,
								arma::colvec 		&esy,
								arma::colvec 		&ssx,
								arma::colvec 		&ssy,
								arma::colvec 		&ry1,
								arma::colvec 		&rx,
								arma::colvec 		&ry2,
								arma::colvec 		&rz,
								arma::colvec 		&tx,
								arma::colvec 		&ty,
								arma::colvec 		&tz
							);
		void bundleAdjustment(	arma::colvec		&xout,		// 3x1 vector
								arma::colvec		&sout,		// 3x1 vector
								arma::colvec		&x0,		// 3x1 vector
								arma::colvec		&centerA,	// 2x1 vector
								arma::colvec		&centerB,	// 2x1 vector
								arma::colvec		&sA,
								arma::colvec		&sB,
								arma::colvec		&c,			// Kamerakonstante -> Brennweite
								arma::colvec 		&a1,
								arma::colvec 		&a2,
								arma::colvec 		&a3,
								arma::colvec 		&b1,
								arma::colvec 		&b2,
								arma::colvec 		&c1,
								arma::colvec 		&c2,
								arma::colvec 		&ppx,
								arma::colvec 		&ppy,
								arma::colvec 		&r0,
								arma::colvec 		&esx,
								arma::colvec 		&esy,
								arma::colvec 		&ssx,
								arma::colvec 		&ssy,
								arma::colvec 		&ry1,
								arma::colvec 		&rx,
								arma::colvec 		&ry2,
								arma::colvec 		&rz,
								arma::colvec 		&tx,
								arma::colvec 		&ty,
								arma::colvec 		&tz,
								int					max_iterations
							);
							
		arma::colvec function (	arma::colvec		&x,		// Positionshypothese
								arma::colvec		&c,		// Kamerakonstante / Brennweite
								std::vector<arma::mat> &R,   // Rotationsmatrix
								std::vector<arma::colvec> &T // Translationsvektor Projektionszentrum
							   );
						
		arma::mat jacobian (	arma::colvec		&x,		// Positionshypothese
								arma::colvec		&c,		// Kamerakonstante / Brennweite
								std::vector<arma::mat> &R,   // Rotationsmatrix
								std::vector<arma::colvec> &T // Translationsvektor Projektionszentrum
							);
								
		
		void correctDistortion(	double &x,	// x-Koordinate
								double &y,	// y-Koordinate
								double &a1,  // Rest: Verzeichnungsparameter
								double &a2, 
								double &a3, 
								double &b1, 
								double &b2, 
								double &c1, 
								double &c2, 
								double &ppx, 
								double &ppy, 
								double &r0,
								double &ssx,
								double &ssy,
								double &esx,
								double &esy
							);
							
		/// Assigns measured marker points to their correspoding model points.					
		void modelToMarkerCorrespondence( arma::mat &xmarker, arma::mat &xmodel, arma::mat &xmarker_new, arma::mat &xmodel_new, arma::mat &correspondence, arma::colvec &correspondenceExists);
		void modelToMarkerCorrespondence( arma::mat &xmarker, arma::mat &xmodel, arma::mat &xmarker_new, arma::mat &xmodel_new);
		void modelToMarkerCorrespondence( arma::mat &xmarker, arma::mat &xmodel, arma::mat &correspondence, arma::colvec &correspondenceExists);
	}

	namespace Locator {
		/// Calculates the locator pose by means of a least squares optimization.
		void locatorPose( arma::mat &xmarker, arma::mat &smarker, arma::mat &xmodel, arma::colvec &xlocator, arma::colvec &slocator, bool &did_converge);
		/// Calculates the locator pose by means of a least squares optimization.
		void locatorPose( arma::mat &xmarker, arma::mat &smarker, arma::mat &xmodel, arma::colvec &xlocator, arma::colvec &slocator, bool &did_converge, int max_iterations);
		/// Calculates the locator pose by means of a least squares optimization.
		// changed due to new function call in locator.cpp by Gca, 2012-06-11
		// void locatorPose( arma::mat &xmarker, arma::mat &smarker, arma::mat &xmodel, arma::colvec &xlocator, arma::colvec &slocator, arma::colvec &xlocator0, bool &did_converge, int max_iterations);
		void locatorPose(arma::mat &xmarker, arma::mat &smarker, arma::mat &xmodel, arma::colvec &xlocator, arma::colvec &slocator, arma::colvec &xlocator0, bool &did_converge, int max_iterations, arma::colvec &f, arma::colvec &f_temp, double &err);

		/// Determine an initial pose estimate.
		void locatorInitialPose(arma::mat &xmarker, arma::mat &xmodel, arma::colvec &xlocator);
		arma::mat convertAxisAngle2RotationMatrix(arma::colvec n, double alpha);
		arma::colvec convertRotationMatrix2UnitQuaternion(arma::mat R);
	}
	
}// namespace OTPF

#endif